﻿/*
 * Copyright © 2020, Wuxi Safover Intelligent Technology Co., Ltd. All rights reserved.
 * 
 * 无锡安永智能科技有限公司版权所有，未经许可，不得用于商业用途
 * 
 * Visit: https://www.safover.com/ 
 */

using log4net;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO.Ports;
using System.Reflection;
using System.Threading;
using Safover.LidarCollector.Core;

namespace Rt.VTH50
{
    public class VTH50RotaryTableDevice : RotaryTableDevice
    {
        private static readonly ILog Log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);

        #region Singleton
        public static RotaryTableDevice Instance { get; } = new VTH50RotaryTableDevice();

        private VTH50RotaryTableDevice()
        {
            PortName = "COM4";

            BaudRate = 9600;

            DataBits = 8;

            Parity = Parity.None;

            StopBits = StopBits.One;

            Handshake = Handshake.None;
        }

        #endregion

        private bool IsMoving { get; set; }

        private bool IsAngleSetDone { get; set; }

        public override string DeviceType => "VTH50RotaryTableDevice";

        protected override void OnDataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            List<byte> stream = new List<byte>();

            while (BytesToRead > 0)
            {
                var buffer = new byte[BytesToRead];

                if (!Read(buffer, 0, buffer.Length))
                {
                    Log.Error($"{DeviceType}: BytesToRead={BytesToRead}, failed to read all bytes");
                }

                stream.AddRange(buffer);
            }

            float? vAngle = null;
            float? hAngle = null;
            for (int i = 0; i <= stream.Count - 7;)
            {
                var head = stream[i];
                var add = stream[i + 1];
                var command = stream[i + 3];

                var b4 = stream[i + 4];
                var b5 = stream[i + 5];

                if (head != 0xFF || add != 0x01)
                {
                    i++;
                    continue;
                }
                else
                {
                    i += 7;
                }

                if (command == (byte)CommandType.UpHorizontalAngle)
                {
                    hAngle = BitConverter.ToUInt16(new byte[] { b5, b4 }, 0) / 100f;
                    hAngle = (float)Math.Round((float)hAngle, 2);
                }
                else if (command == (byte)CommandType.UpVerticalAngle)
                {
                    vAngle = (360 - BitConverter.ToUInt16(new byte[] { b5, b4 }, 0) / 100f) % 360;
                    vAngle = (float)Math.Round((float)vAngle, 2);
                }
                else if (command == (byte)CommandType.AngleSetDone)
                {
                    IsAngleSetDone = true;
                }
            }

            VerticalAngle = vAngle == null ? VerticalAngle : vAngle;
            HorizontalAngle = hAngle == null ? HorizontalAngle : hAngle;

            InvokeReadingStatusChangedEvent();
        }


        public override bool MoveHorizontallyTo(float angle)
        {
            return Move(CommandType.HorizontalAngleSet, angle);
        }

        public override bool MoveVerticallyTo(float angle)
        {
            return Move(CommandType.VerticalAngleSet, angle);
        }

        private int PredictTimeInSeconds(CommandType commandType, float targetAngle)
        {
            const int maxAllowSeconds = 30;
            float range;
            if (commandType == CommandType.VerticalAngleSet)
            {
                if (VerticalAngle == null) return maxAllowSeconds;

                range = Math.Abs((float)VerticalAngle - targetAngle);
            }
            else 
            {
                if (HorizontalAngle == null) return maxAllowSeconds;

                range = Math.Abs((float)HorizontalAngle - targetAngle);
            }

            int timeout = (int)(range * 0.2);
            return (timeout > maxAllowSeconds) ? maxAllowSeconds : timeout;
        }

        private bool Move(CommandType commandType, float angle)
        {
            if (IsMoving) return false;

            IsAngleSetDone = false;

            int timeout = PredictTimeInSeconds(commandType, angle);

            IsMoving = true;

            VTH50RotaryTableCommand command = VTH50RotaryTableCommandBuilder.BuildCommand(commandType, angle);
            InvokeReadingStatusChangedEvent();

            try
            {
                Write(command.CommandBytes);

                Stopwatch stopwatch = new Stopwatch();
                stopwatch.Start();
                TimeSpan timeAllowed = TimeSpan.FromSeconds(timeout);
                
                do
                {
                    Thread.Sleep(1000);

                    if (IsAngleSetDone) break;

                } while (stopwatch.Elapsed < timeAllowed);

                switch (commandType)
                {
                    case CommandType.VerticalAngleSet:
                        VerticalAngle = angle;
                        break;
                    case CommandType.HorizontalAngleSet:
                        HorizontalAngle = angle;
                        break;
                    default:
                        break;
                }

                return true;
            }
            finally
            {
                IsMoving = false;
                InvokeReadingStatusChangedEvent();
            }
        }

        private void InvokeReadingStatusChangedEvent()
        {
            try
            {
                DeviceReadingsUpdated?.Invoke(this, new RotaryTableReadingsUpdatedEventArgs
                {
                    VerticalAngle = VerticalAngle,
                    HorizontalAngle = HorizontalAngle,
                    IsRotaryTableMoving = IsMoving,
                });
            }
            catch { }
        }

        public override void StopMove()
        {
            var command = VTH50RotaryTableCommandBuilder.BuildStopCommand();
            Write(command.CommandBytes);
        }

        public override bool RestoreZero()
        {
            throw new NotImplementedException();
        }

        public override void QueryHorizontalAngle()
        {
            var command = VTH50RotaryTableCommandBuilder.BuildCommand(CommandType.HorizontalAngleQuery, 0);
            Write(command.CommandBytes);
        }

        public override void QueryVerticalAngle()
        {
            var command = VTH50RotaryTableCommandBuilder.BuildCommand(CommandType.VerticalAngleQuery, 0);
            Write(command.CommandBytes);
        }
    }
}
